#include <stdio.h>



typedef struct
	{
		str_matrix A;
		str_matrix A_t;
		str_matrix B;
		str_matrix C;
		str_matrix C_t;
		str_matrix D;
		str_matrix P;
		str_matrix I;
		str_matrix Ex;
		str_matrix Ez;
		
		double Ts;
		
		str_matrix x;

	}str_kalman;


str_kalman create_kalman(str_matrix * A_,str_matrix * B_,str_matrix * C_,str_matrix * D_
	,str_matrix * P_,double Ts_,str_matrix * Ex_, str_matrix * Ez_){
	
	str_kalman kalm;
	
	kalm.A=*A_;
	kalm.A_t=transpuesta(A_);
	kalm.B=*B_;
	kalm.C=*C_;
	kalm.C_t=transpuesta(C_);
	kalm.D=*D_;
	kalm.P=*P_;
	kalm.I=eye(A_->fila);
	kalm.Ex=*Ex_;
	kalm.Ez=*Ez_;
	
	kalm.Ts=Ts_;

	kalm.x=zeros(A_->fila,1);

	return kalm;
}

str_matrix kalman_step(str_kalman * kalm, double u,str_matrix * z){
	double auxiliar;
	str_matrix x_;
	str_matrix S;
	str_matrix invS;
	str_matrix K;
	str_matrix aux1;
	str_matrix aux2;

	/* PASO 1: x=A*x + B*u */
	aux1=prod(&kalm->A,&kalm->x);
	aux2=prod_escalar(&kalm->B,u);
	x_=suma(&aux1,&aux2); 
	destroy_matrix(&aux1);
	destroy_matrix(&aux2);

	/* PASO 2: P=A*P*A' + Ex */
	aux1=prod(&kalm->A,&kalm->P);
	aux2=prod(&aux1,&kalm->A_t);
	destroy_matrix(&kalm->P);
	kalm->P=suma(&aux2,&kalm->Ex);
	destroy_matrix(&aux1);
	destroy_matrix(&aux2);
	
	/* PASO 3: S=C*P*C' + Ez */
	S=prod(&kalm->C,&kalm->P);
	prod_2(&S,&kalm->C_t);
	suma_2(&S,&kalm->Ez);
	
	/* PASO 4: K=P*C'*inv(S) */
	invS=inversa(&S);
	destroy_matrix(&S);
	K=prod(&kalm->P,&kalm->C_t);
	prod_2(&K,&invS);
	destroy_matrix(&invS);
	
	/* PASO 5: x=x + K*(z(i) - H*x) */
	aux1=prod(&kalm->C,&x_);
	auxiliar=(*(z->data +1))-(*(aux1.data));
	/*resta_2(*(z->data +1),&aux1);*/
	aux2=prod_escalar(&K,auxiliar);
	suma_2(&x_,&aux2);
	destroy_matrix(&aux1);
	destroy_matrix(&aux2);
	set_values(&kalm->x,x_.data);
	
	/* PASO 6: P=(I-K*C)*P */
	prod_2(&K,&kalm->C);
	aux1=resta(&kalm->I,&K);
	aux2=prod(&aux1,&kalm->P);
	set_values(&kalm->P,aux2.data);
	destroy_matrix(&aux1);
	destroy_matrix(&aux2);

	return x_;
}

void restore_P(str_kalman * k,double * Ex){
	set_values(&k->P,Ex);
}
